/***********************************************************************************
 * 文件名： ly_pid.c
 * 作者： 	刘言
 * 版本： 	1
 * 说明：
 * 		面相对象设计的PID算法计算器，注意不要与单例模式的版本 pid_static 同时使用，会出
 * 现命名冲突。
 * 		PID 计算方法： 控制（输出）值的变化量 = 
 * 				  比例系数 * 偏差值 
 * 				+ 积分系数 * 偏差累加值 
 * 				+ 微分系数 * (偏差值 - 上一个偏差值)
 * 		
 * 修改记录：
 * 	20220715：初版。
***********************************************************************************/
#include "ly_pid.h"
#include "polsys.h"

// 确定计算时中间临时变量的类型
#ifdef _PID_USE_FLOAT
	typedef float ctrl_value_pd_t;
	typedef float ctrl_value_i_t;
#else
	#define PID_CTRL_PD_MAX	(_PID_KP_MAX * _PID_INPUT_MAX)
	#define PID_CTRL_I_MAX	(_PID_KI_MAX * _PID_INTE_MAX)

	#if (PID_CTRL_PD_MAX <= 0X7F)
		typedef s8 ctrl_value_pd_t;
	#elif (PID_CTRL_PD_MAX <= 0X7FFF)
		typedef s16 ctrl_value_pd_t;
	#elif (PID_CTRL_PD_MAX <= 0X7FFFFFFF)
		typedef s32 ctrl_value_pd_t;
	#else
		#error "参数设置的不合理导致中间临时变量过大。"
	#endif

	#if (PID_CTRL_I_MAX <= 0X7F)
		typedef s8 ctrl_value_i_t;
	#elif (PID_CTRL_I_MAX <= 0X7FFF)
		typedef s16 ctrl_value_i_t;
	#elif (PID_CTRL_I_MAX <= 0X7FFFFFFF)
		typedef s32 ctrl_value_i_t
	#else
		#error "参数设置的不合理导致中间临时变量过大。"
	#endif
#endif





void PID_init(ly_pid_t this, pid_kp_t kp, pid_ki_t ki, pid_kd_t kd)
{
	this->mKp = kp;
	this->mKi = ki;
	this->mKd = kd;
	this->mIntegralValueMax = _PID_MAX_INTE_VALUE;
	this->mIntegralValueMin = _PID_MIN_INTE_VALUE;
	PID_reset(this);
}

void PID_reset(ly_pid_t this)
{
	this->mLastDeviationValue = 0;
	this->mIntegralValue = 0;
	#ifdef _PID_USE_DYNAMIC_INTEGRAL	// 启用动态积分
	this->mArrive = false;
	#endif
}


ctrl_value_t PID_getChange(ly_pid_t this, input_value_t target, input_value_t real)
{
	ctrl_value_t change;			// 控制量变化值
	input_value_t deviation_value;	// 偏差值
#ifdef _PID_USE_DYNAMIC_INTEGRAL	// 启用动态积分[待实现]
	u8 IntegralRatio;		// 积分起作用的比例值
	
	deviation_value = target - real;	//偏差值，有符号
	#ifdef _PID_NO_INTEGRAL_IN_START		//启动时关闭积分作用
	if(mArrive == false)	//本次启动没有达到过目标值
	{
		IntegralRatio=0;		//关闭积分作用
		if(deviation_value<0)mArrive=true;	//标记达到目标值了
	}
	else
	#endif
	{
		// u16 temp;
		// temp = GetAbsS16(deviation_value);	//偏差绝对值
		// if(temp>_PID_NO_INTEGRAL_VALUE)		//偏差过大关闭积分作用
		// {
		// 	IntegralRatio=0;
		// }
		// else if(temp<_PID_FULL_INTEGRAL_VALUE)//偏差很小，正常积分
		// {
			IntegralRatio=1;
			mIntegralValue += deviation_value;
		// }
		// else		//偏差偏大，积分减弱
		// {
		// 	IntegralRatio=(_PID_NO_INTEGRAL_VALUE-temp)*_PID_SCALE/(_PID_NO_INTEGRAL_VALUE-_PID_FULL_INTEGRAL_VALUE);
		// 	mIntegralValue += deviation_value;					//累加偏差值，有符号
		// }
	}
#else 	// 关闭动态积分，始终完全积分
	deviation_value = target - real;	// 偏差值，有符号
	this->mIntegralValue += deviation_value;	// 累加偏差值，有符号
#endif
	// 限制积分饱和
	if(this->mIntegralValue > this->mIntegralValueMax) this->mIntegralValue = this->mIntegralValueMax;
	if(this->mIntegralValue < this->mIntegralValueMin) this->mIntegralValue = this->mIntegralValueMin;
	// 计算控制值
#ifdef _PID_USE_DYNAMIC_INTEGRAL
	#ifdef _PID_USE_FLOAT
		change = ((ctrl_value_pd_t)_PID_KP * deviation_value
				+ (ctrl_value_i_t)_PID_KI * mIntegralValue * IntegralRatio
				+ (ctrl_value_pd_t)_PID_KD * (deviation_value - mLastDeviationValue)
				);
	#else
		change = ((ctrl_value_pd_t)_PID_KP * deviation_value
		#if (_PID_KI != 0)
				+ (ctrl_value_i_t)_PID_KI * mIntegralValue * IntegralRatio
		#endif
		#if (_PID_KD != 0)
				+ (ctrl_value_pd_t)_PID_KD * (deviation_value - mLastDeviationValue)
		#endif
				) / _PID_SCALE;
	#endif
#else
	#ifdef _PID_USE_FLOAT
		change = ((ctrl_value_pd_t)this->mKp * deviation_value
				+ (ctrl_value_i_t)this->mKi * this->mIntegralValue
				+ (ctrl_value_pd_t)this->mKd * (deviation_value - this->mLastDeviationValue)
				);
	#else
		change = ((ctrl_value_pd_t)this->mKp * deviation_value
				+ (ctrl_value_i_t)this->mKi * this->mIntegralValue
				+ (ctrl_value_pd_t)this->mKd * (deviation_value - this->mLastDeviationValue)
				) / _PID_SCALE;
	#endif
#endif
	// 保存本次差值
	this->mLastDeviationValue = deviation_value;	

	return change;
}



